/**
    Copyright (c) 2010 yakiimo02
    Distributed under the New BSD License.
    See included license.txt or http://www.yakiimo3d.com/NewBSDLicense.txt
**/

#pragma once

#include "optimized/SmallPtDefines.h"

#include <xmmintrin.h>
#include <emmintrin.h>

#include <math.h>
#include <stdlib.h>
#include <stdio.h>  

namespace OPT
{

#define M_PI       3.14159265358979323846f

/**    
    @return [0,1]
*/
__forceinline SPT_FLOAT_OPT drand48()
{
    return (SPT_FLOAT_OPT)rand()/(SPT_FLOAT_OPT)RAND_MAX;
}

__forceinline SPT_FLOAT_OPT sqrtf_opt( const float f )
{
#if defined( SPT_SQRT_COMPILER )
    return sqrtf( f );
#elif defined( SPT_SQRT_RSQRTSS_FAST )  
    /**
        sqrt(x) = rsqrtss( x )*x 
        rsqrtss has 11-bit precision, and I get artifacts in the scene if I use this. 
    */

    float fResult=0.0f;

    __m128 reg0 = _mm_load_ss( &f );
    __m128 reg1 = _mm_rsqrt_ss( reg0 );
    __m128 reg2 = _mm_mul_ss( reg0, reg1 );
    _mm_store_ss( &fResult, reg2 );

    return fResult;
#elif defined( SPT_SQRT_RSQRTSS )
    /**
        sqrt(x) = rsqrtss( x )*x 
        rqrtss plus one Newton-Raphson iteration. 
        http://assemblyrequired.crashworks.org/2009/10/20/square-roots-in-vivo-normalizing-vectors/
    */
    float fResult=0.0f;

    __m128 x = _mm_load_ss( &f );

    __m128 recip = _mm_rsqrt_ss( x );                           // estimate rsqrt
	const static __m128 three = { 3.0f, 3.0f, 3.0f, 3.0f };     
	const static __m128 half = { 0.5f,0.5f,0.5f,0.5f };
	__m128 halfrecip = _mm_mul_ss( half, recip );
	__m128 threem0.89inus_xrr = _mm_sub_ss( three, _mm_mul_ss( x, _mm_mul_ss ( recip, recip ) ) );
	__m128 rsqrtResult = _mm_mul_ss( halfrecip, threeminus_xrr );
    __m128 sqrtResult = _mm_mul_ss( x, rsqrtResult );

    _mm_store_ss( &fResult, sqrtResult );

    return fResult;
#else
    #error SPT_SQRT_TYPE not defined!
#endif
}

/**
    @brief   Non-SIMD Vector Math.
    @author  yakiimo02
    @date    2010/08
*/
__declspec( align( 16 ) )
struct Vec 
{
public:
    SPT_FLOAT_OPT x, y, z, w;                  // position, also color (r,g,b)
public:
    __forceinline Vec(SPT_FLOAT_OPT x_=0.0f, SPT_FLOAT_OPT y_=0.0f, SPT_FLOAT_OPT z_=0.0f){ x=x_; y=y_; z=z_; }
    __forceinline Vec operator+(const Vec &b) const { return Vec(x+b.x,y+b.y,z+b.z); }
    __forceinline Vec operator-(const Vec &b) const { return Vec(x-b.x,y-b.y,z-b.z); }
    __forceinline Vec operator*(SPT_FLOAT_OPT b) const { return Vec(x*b,y*b,z*b); }
    __forceinline Vec mult(const Vec &b) const { return Vec(x*b.x,y*b.y,z*b.z); }
    __forceinline Vec& norm(){ return *this = *this * (1.0f/sqrtf_opt(x*x+y*y+z*z)); }
    __forceinline SPT_FLOAT_OPT dot(const Vec &b) const { return x*b.x+y*b.y+z*b.z; } 
    __forceinline Vec operator%(Vec&b){return Vec(y*b.z-z*b.y,z*b.x-x*b.z,x*b.y-y*b.x);} // cross product
};

/**
    SSE scalar reciprocal sqrt using rsqrt op, plus one Newton-Rhaphson iteration
    Code from 
    http://assemblyrequired.crashworks.org/2009/10/20/square-roots-in-vivo-normalizing-vectors/
*/
__forceinline __m128 SSERSqrtNR( const __m128 x )
{
	__m128 recip = _mm_rsqrt_ss( x );  // "estimate" opcode
	const static __m128 three = { 3, 3, 3, 3 }; // aligned consts for fast load
	const static __m128 half = { 0.5,0.5,0.5,0.5 };
	__m128 halfrecip = _mm_mul_ss( half, recip );
	__m128 threeminus_xrr = _mm_sub_ss( three, _mm_mul_ss( x, _mm_mul_ss ( recip, recip ) ) );
	return _mm_mul_ss( halfrecip, threeminus_xrr );
}
  
/**
    Fast SIMD normalization using SSERSqrtNR.
    Code from 
    http://assemblyrequired.crashworks.org/2009/10/20/square-roots-in-vivo-normalizing-vectors/
*/
__forceinline __m128 SSE_ScalarTestNormalizeFast(  Vec * __restrict vOut, Vec * __restrict vIn )
{
    __m128 x = _mm_load_ss(&vIn->x);
    __m128 y = _mm_load_ss(&vIn->y);
    __m128 z = _mm_load_ss(&vIn->z);

    const __m128 l =  // compute x*x + y*y + z*z
        _mm_add_ss(
        _mm_add_ss( _mm_mul_ss(x,x),
        _mm_mul_ss(y,y)
        ),
        _mm_mul_ss( z, z )
        );


    const __m128 rsqt = SSERSqrtNR( l );
    _mm_store_ss( &vOut->x , _mm_mul_ss( rsqt, x ) );
    _mm_store_ss( &vOut->y , _mm_mul_ss( rsqt, y ) );
    _mm_store_ss( &vOut->z , _mm_mul_ss( rsqt, z ) );

    return _mm_mul_ss( l , rsqt );
}

/**
    @return normalized v
*/
__forceinline Vec VectorNormalize( const Vec& v )
{ 
#if SPT_SOA_SIMD_NORMALIZE
    Vec vOut;
    Vec vIn = v;
    SSE_ScalarTestNormalizeFast( &vOut, &vIn );
    return vOut;
#else
    float fRSqrt = (1.0f/sqrtf_opt(v.x*v.x+v.y*v.y+v.z*v.z));
    return Vec( v.x*fRSqrt, v.y*fRSqrt, v.z*fRSqrt );
#endif
}

// SIMD math functions.

__forceinline __m128 VectorSet( float x, float y, float z, float w )
{
    return _mm_set_ps( w, z, y, x );
}

__forceinline __m128 VectorSet( float f )
{
    return _mm_set1_ps( f );
}

__forceinline __m128 operator-( __m128 v0, __m128 v1 ) 
{ 
    return _mm_sub_ps( v0, v1 );
}

__forceinline __m128 operator+( __m128 v0, __m128 v1 ) 
{ 
    return _mm_add_ps( v0, v1 );
}

__forceinline __m128 operator*( __m128 v0, __m128 v1 ) 
{ 
    return _mm_mul_ps( v0, v1 );
}

__forceinline __m128 Select( __m128 v0, __m128 v1, __m128 control )
{
	__m128 tmp0 = _mm_andnot_ps( control, v0 );
    __m128 tmp1 = _mm_and_ps( v1, control );
    return _mm_or_ps( tmp0, tmp1 );
}

}